#include "ctrl.h"
#include "oslib_can.h"
#include "oslib_uart.h"
#include "motorTask.h"
#include "command.h"
#include "oslib_debug.h"
#include "config.h"
#include "math.h"
#include "cmsis_os2.h"
#include "motorflash.h"

int maxPosVel[4];
MotorType motors[4];
DriverType drivers[4];

MotorType_TypeDef motorType[4] = {
    RM_3508,
    M_2006,
    M_2006,
    M_2006,
};

ControlMode motorControlMode[4] = {
    SPEED_CONTROL_MODE,
    POSITION_CONTROL_MODE,
    POSITION_CONTROL_MODE,
    POSITION_CONTROL_MODE,
};

CurveObjectType motorSigmoidVel[4]; // 控制位置环电机加减速
/**
 * @brief  3508电机初始化
 * @param {DriverType} *driver
 * @param {uint8_t} mode
 * @return {*}
 */
void RM_3508_init(DriverType *driver, uint8_t mode)
{
    memset(&(driver->posCtrl), 0, sizeof(PosCtrlType));
    memset(&(driver->curCtrl), 0, sizeof(CurCtrlType));
    memset(&(driver->velCtrl), 0, sizeof(VelCtrlType));
    driver->controlMode = mode;

    driver->velCtrl.speed_pid = (PID_Struct){VEL_KP_3508, 0, VEL_KI_3508, 0, 0, 12, 0, 0.001};
    driver->velCtrl.maxOutput = CURRENT_MAX_3508;
    driver->velCtrl.maxSpeed = VEL_MAX_3508;

    driver->posCtrl.pos_pid = (PID_Struct){POS_KP_3508, POS_KD_3508, 0, 0, 0, 0, 0, 0.001};

    driver->curCtrl.maxCur = CURRENT_MAX_3508;
    driver->homingMode.current = 1.0f;

    driver->posCtrl.PosVel = 50.0f;

    driver->homingMode.vel = -50.0f;
    driver->period = 8192;

    driver->velCtrl.leftLimit = -100000;
    driver->velCtrl.rightLimit = -20000;
}
/**
 * @brief  2006电机初始化
 * @param {DriverType} *driver
 * @param {uint8_t} mode
 * @return {*}
 */
void M_2006_init(DriverType *driver, uint8_t mode)
{
    memset(&(driver->posCtrl), 0, sizeof(PosCtrlType)); // 申请内存
    memset(&(driver->curCtrl), 0, sizeof(CurCtrlType));
    memset(&(driver->velCtrl), 0, sizeof(VelCtrlType));
    driver->controlMode = mode;
    // 速度环配置
    driver->velCtrl.speed_pid = (PID_Struct){VEL_KP_2006, 0, VEL_KI_2006, 0, 0, 2, 0, 0.001};
    driver->velCtrl.maxOutput = CURRENT_MAX_2006;
    driver->velCtrl.maxSpeed = VEL_MAX_2006;
    // 位置环配置
    driver->posCtrl.pos_pid = (PID_Struct){POS_KP_2006, POS_KD_2006, 0, 0, 0, 5, 0, 0.001};

    driver->curCtrl.maxCur = CURRENT_MAX_2006; // 电流限幅
    driver->homingMode.current = 1.0f;

    driver->posCtrl.PosVel = 50.0f; // 位置环默认速度

    driver->homingMode.vel = -50.0f; // homing mode 速度
    driver->period = 8192;           // 角度范围 0-8191

    driver->posCtrl.arrivalFlag = COMMON;
}
/**
 * @brief  6020电机初始化
 * @param {DriverType} *driver
 * @param {uint8_t} mode
 * @return {*}
 */
void GM_6020_init(DriverType *driver, uint8_t mode)
{
    memset(&(driver->posCtrl), 0, sizeof(PosCtrlType));
    memset(&(driver->curCtrl), 0, sizeof(CurCtrlType));
    memset(&(driver->velCtrl), 0, sizeof(VelCtrlType));
    driver->controlMode = mode;

    driver->velCtrl.speed_pid = (PID_Struct){VEL_KP_6020, 0, VEL_KI_6020, 0, 0, 2, 0, 0.001};
    driver->velCtrl.maxOutput = VOLTAGE_MAX_6020;
    driver->velCtrl.maxSpeed = VEL_MAX_6020;

    driver->posCtrl.pos_pid = (PID_Struct){POS_KP_6020, POS_KD_6020, 0, 0, 0, 2, 0, 0.001};
    driver->curCtrl.maxCur = VOLTAGE_MAX_6020;
    driver->homingMode.current = 1.0f;

    driver->posCtrl.PosVel = 30.0f;

    driver->homingMode.vel = -50.0f;
    driver->period = 8192;
}

/**
 * @brief  初始化驱动器
 * @param  None
 * @retval
 */
void DriverInit(void)
{
    // motor_WriteParam();
    motor_ReadParam(); // 读取FLASH上的参数
    for (int i = 0; i < 4; i++)
    {
        uprintf("motorType[%d] = %d\r\n", i, motorType[i]);
    }
    for (int i = 0; i < 4; i++)
    {
        uprintf("motorCtrlMode[%d] = %d\r\n", i, motorControlMode[i]);
    }
    for (int i = 0; i < 4; i++)
    {
        uprintf("maxPosVel[%d] = %d\r\n", i, maxPosVel[i]);
    }

    for (int i = 0; i < 4; i++)
    {
        drivers[i].command.canId = i + 1;
        motors[i].type = motorType[i];
        if (motors[i].type == RM_3508)
        {
            RM_3508_init(&drivers[i], motorControlMode[i]);
            MotorOn(i);
        }

        else if (motors[i].type == M_2006)
        {
            M_2006_init(&drivers[i], motorControlMode[i]);
            MotorOn(i);
        }
        else if (motors[i].type == GM_6020)
        {
            GM_6020_init(&drivers[i], motorControlMode[i]);
            MotorOn(i);
        }
        else
        {
            MotorOff(i);
            continue;
        }
        if (motorControlMode[i] == POSITION_CONTROL_MODE)
        {
            drivers[i].posCtrl.PosVel = maxPosVel[i];
        }
    }
}

float perCur[4] = {0.0}; // 发给电机的电流值
float wave[4];
int waveNum = 0;
float desireSpeed[4] = {0.0f};
float posDelta[4] = {0.0f};

extern osMutexId_t motorsMutexHandle;
/**
 * @brief  电机控制， 根据controlMode设置电机控制模式，根据电机反馈更新driver速度位置信息
 * @param None
 */
void MotorCtrl(void)
{
    osMutexAcquire(motorsMutexHandle, osWaitForever);
    for (int i = 0; i < 4; i++)
    {
        if (motors[i].type == NONE)
            continue;
        UpdateSpeedPos(&drivers[i], &motors[i]);
    }
    osMutexRelease(motorsMutexHandle);

    for (int i = 0; i < 4; i++)
    {
        if (motors[i].type == NONE)
            continue;

        if (drivers[i].status != ENABLE)
        {
            drivers[i].output = 0;
            continue;
        }
        switch (drivers[i].controlMode)
        {
        case POSITION_CONTROL_MODE:
            drivers[i].velCtrl.desireSpeed = PosCtrl(&drivers[i].posCtrl, i);
            drivers[i].output = VelCtrl(&drivers[i].velCtrl);

            desireSpeed[i] = drivers[i].velCtrl.desireSpeed;
            posDelta[i] = fabsf(drivers[i].posCtrl.desirePos - drivers[i].posCtrl.actulPos);
            // if (i == 0)
            // {
            //     uprintf("old output: %f\r\n", drivers[i].output);
            // }
            // drivers[i].output = SigmoidPosSpeed(&drivers[i]);
            break;
        case SPEED_CONTROL_MODE:
            drivers[i].output = VelCtrl(&drivers[i].velCtrl);
            break;
        case CUR_CONTROL_MODE:
            drivers[i].output = drivers[i].curCtrl.desireCur;
            // Info("%d %d %d",i,(int)drivers[i].output,(int)drivers[i].curCtrl.desireCur);
            break;
        case HOMING_MODE:
            drivers[i].output = HomingMode(&drivers[i]);
            break;
        case SPEED_LIMIT_MODE:
            drivers[i].output = SpeedLimitMode(&drivers[i].velCtrl, drivers[i].posCtrl.actulPos);
        default:
            break;
        }
    }
    static int putcnt = 0;
    // 根据 PID输出速度值转为电流值
    for (int i = 0; i < 4; i++)
    {
        if (motors[i].type == RM_3508)
            perCur[i] = drivers[i].output * 16384.0f / 20.0f;
        else if (motors[i].type == M_2006)
            perCur[i] = drivers[i].output * 10000.0f / 10.0f; // M2006
        else if (motors[i].type == GM_6020)
            perCur[i] = drivers[i].output * 30000.0f / 30.f;
        else
            perCur[i] = 0.0f;
    }
    // 将电流值发送至电机
    SetCur(perCur);
    // 示波器数据发送
    putcnt++;
    if (waveNum && putcnt % 20 == 0) //
    {
        wave[0] = drivers[waveNum - 1].velCtrl.actualSpeed;
        wave[1] = drivers[waveNum - 1].posCtrl.actulPos;
        wave[2] = drivers[waveNum - 1].velCtrl.actualSpeed;
        wave[3] = perCur[waveNum - 1];
        toolBox_scope(wave, 4);
    }
}
/**
 * @brief  更新电机的当前速度和位置，将速度环和位置环信息一起更新
 * @param driver 需要更新的受控电机
 * @param motor 接受的电机报文信息
 */
void UpdateSpeedPos(DriverType *driver, MotorType *motor)
{
    int deltaPos;
    deltaPos = (motor->pos - motor->posLast);
    motor->posLast = motor->pos;
    // 正确记录电机反转时的实际角度 从 1 反转到 8911，只转了 2
    if (deltaPos > (driver->period / 2))
        deltaPos -= driver->period;
    if (deltaPos < -(driver->period / 2))
        deltaPos += driver->period;
    driver->posCtrl.actulPos += deltaPos;

    driver->velCtrl.actualSpeed = (float)(motor->vel) * 0.1365333f; // 1/60*8912/1000 转子转速 rpm -> 电机转速 r/ms
}
/**
 * @brief  通过CAN2反馈主控，电机位置环已到达
 * @param  motorId {0 1 2 3}
 */
void InformPosArrival(int32_t motorId)
{
    uprintf("motorId is %d\r\n", motorId);
    int canId = 0x286;
    int16_t boardId = BOARDID;
    can_msg msg;
    msg.i16[0] = boardId;
    msg.i16[1] = motorId + 1;

    OSLIB_CAN_SendMessage(&hcan2, CAN_ID_STD, canId, &msg);
}

float PosCtrl(PosCtrlType *posPid, int i)
{
    float posPidOut;

    if (posPid->arrivalInform && fabsf(posPid->desirePos - posPid->actulPos) < 100)
    {
        posPid->arrivalInform = 0;
        InformPosArrival(i);
    }

    posPidOut = PID_Release(&posPid->pos_pid, posPid->desirePos, posPid->actulPos);

    posPid->output = MaxMinLimit(posPidOut, posPid->PosVel);

    return posPid->output; // 输出预期速度
}

float VelCtrl(VelCtrlType *velPid)
{
    float velPidOut;

    velPidOut = PID_Release(&velPid->speed_pid, velPid->desireSpeed, velPid->actualSpeed);

    velPid->output = MaxMinLimit(velPidOut, velPid->maxOutput);

    return velPid->output;
}

float SpeedLimitMode(VelCtrlType *velPid, float pos)
{
    float velPidOut;

    if ((pos < velPid->leftLimit - 1000) && velPid->desireSpeed < 0)
        velPidOut = PID_Release(&velPid->speed_pid, -velPid->desireSpeed, velPid->actualSpeed);
    else if (pos < velPid->leftLimit && velPid->desireSpeed < 0)
        velPidOut = PID_Release(&velPid->speed_pid, 0, velPid->actualSpeed);
    else if (pos > velPid->rightLimit + 1000 && velPid->desireSpeed > 0)
        velPidOut = PID_Release(&velPid->speed_pid, -velPid->desireSpeed, velPid->actualSpeed);
    else if (pos > velPid->rightLimit && velPid->desireSpeed > 0)
        velPidOut = PID_Release(&velPid->speed_pid, 0, velPid->actualSpeed);
    else
        velPidOut = PID_Release(&velPid->speed_pid, velPid->desireSpeed, velPid->actualSpeed);

    velPid->output = MaxMinLimit(velPidOut, velPid->maxOutput);

    return velPid->output;
}

void InformHomingOver(int32_t motorId)
{
    uprintf("motorId is %d\r\n", motorId);
    int canId = 0x285;
    int16_t boardId = BOARDID;
    can_msg msg;
    msg.i16[0] = boardId;
    msg.i16[1] = motorId + 1;

    OSLIB_CAN_SendMessage(&hcan2, CAN_ID_STD, canId, &msg);
}

float HomingMode(DriverType *driver)
{
    float output;

    driver->velCtrl.desireSpeed = driver->homingMode.vel;

    output = VelCtrl(&driver->velCtrl);

    output = MaxMinLimit(output, driver->homingMode.current);

    if (fabsf(driver->velCtrl.actualSpeed) <= 2)
    {
        driver->homingMode.cnt++;
    }
    else
    {
        driver->homingMode.cnt = 0;
    }
    if (driver->homingMode.cnt >= 500)
    {
        uprintf("homing over!\r\n");
        InformHomingOver((driver - drivers));
        memset(&(driver->homingMode), 0, sizeof(HomingModeType));
        driver->posCtrl.actulPos = 0.0f;
        driver->posCtrl.desirePos = 0.0f;
        driver->velCtrl.desireSpeed = 0.0f;
        driver->velCtrl.output = 0.0f;
        reset_PID(&(driver->velCtrl.speed_pid));
        reset_PID(&(driver->posCtrl.pos_pid));
        driver->controlMode = POSITION_CONTROL_MODE;
        driver->posCtrl.arrivalInform = 0;
    }
    return output;
}

/**
 * @brief  max min limit
 * @param  inDat:
 * @retval outDat
 */
float MaxMinLimit(float val, float limit)
{
    if (val > limit)
        val = limit;
    if (val < -limit)
        val = -limit;
    return val;
}

/**
 * @brief  sigmoid 平滑 位置环加速度
 * @param {DriverType} *driver
 * @return {*}
 */
float SigmoidPosSpeed(DriverType *driver)
{
    static float upInitialPos; // 记录位置环开始的起始位置
    float ratio;
    // 切换为加速阶段
    // TODO 4096 这个值要改为动态的
    if (driver->posCtrl.arrivalFlag == COMMON && fabsf(driver->posCtrl.desirePos - driver->posCtrl.actulPos) > 4096)
    {
        driver->posCtrl.arrivalFlag = UP;
        upInitialPos = driver->posCtrl.actulPos;
    }
    // 切换至减速阶段
    else if (driver->posCtrl.arrivalFlag == UP && fabsf(driver->posCtrl.desirePos - driver->posCtrl.actulPos) < 4096)
    {
        driver->posCtrl.arrivalFlag = DOWN;
    }
    // 减速完成，换回匀速
    else if (driver->posCtrl.arrivalFlag == DOWN && fabsf(driver->posCtrl.desirePos - driver->posCtrl.actulPos) < 100)
    {
        driver->posCtrl.arrivalFlag = COMMON;
    }
    // 该阶段进行加速
    if (driver->posCtrl.arrivalFlag == UP && fabsf(driver->posCtrl.actulPos - upInitialPos) < 4096)
    {
        ratio = driver->posCtrl.PosVel / 4096.0f;
        driver->velCtrl.desireSpeed = ratio * fabsf(driver->posCtrl.actulPos - upInitialPos);
        driver->velCtrl.desireSpeed = MaxMinLimit(driver->velCtrl.desireSpeed, driver->posCtrl.PosVel);
    }
    return 0.0f;
    // 梯形曲线
    if (driver->output < 8.0f / 3.0f)
    {
        driver->output *= 3.0f;
    }
    else if (8.0f / 3.0f < driver->output && driver->output < 16.0f / 3.0f)
    {
        driver->output = driver->output;
    }
    else
    {
        driver->output = 24.0f - 3 * driver->output;
    }
    // 目前以 2006 测试
    // float targetSpeed, ratio, output, delta;
    // // uprintf("~~~~~~~~~~~~~~~~~\r\n");
    // // uprintf("old output: %f\r\n", driver->output);
    // output = 8.0f * sigmoid(driver->output);
    // delta = output - driver->output;
    // driver->output -= delta;
    driver->output = MaxMinLimit(driver->output, driver->velCtrl.maxOutput);
    // uprintf("new output: %f\r\n", driver->output);

    return driver->output;
}

float SpeedMappingRatio(DriverType *driver)
{
    float targetSpeed, ratio;
    targetSpeed = fabs(driver->velCtrl.desireSpeed);
    ratio = targetSpeed / 8; // 把速度从 0-targetSpeed 映射到 0-8 的比例尺
    return ratio;
}
/**
 * @brief sigmoid 处理
 * @param {float} x 映射之后的速度 0-8 之间
 * @return {*}
 */
float sigmoid(float x)
{
    float res = 0;
    uint8_t flag_sign = 0;
    if (x < 0)
    {
        flag_sign = 1;
        x = -x;
    }
    res = -0.004465f * pow(x + 1.2f, 3) + 0.06698f * pow(x + 1.2f, 2) - 0.1366f * (x + 1.2f) + 0.07606f;

    // if (res < 0)
    //     res = 0;

    if (flag_sign == 1)
        return -res;
    else
        return res;
}
/**
 * @brief 电机曲线加减速操作
 * @param {CurveObjectType} *curve
 * @return {*}
 */
void MotorVelCurve(CurveObjectType *curve)
{
    float temp = 0;
    curve->targetSpeed = MaxMinLimit(curve->targetSpeed, curve->speedMax);
    // 加速最大时间为 0 && 速度增量小于预设加速度
    if ((fabs(curve->currentSpeed - curve->startSpeed) <= curve->stepSpeed) && (curve->maxTimes == 0))
    {
        if (curve->startSpeed < curve->speedMin)
        {
            curve->startSpeed = curve->speedMin;
        }
        temp = fabs(curve->targetSpeed - curve->startSpeed);
        temp = temp / curve->stepSpeed;
        curve->maxTimes = (uint32_t)(temp) + 1; // 按照给定加速度设定最大加速时间
        curve->aTimes = 0; // 复位加速时间
    }

    if (curve->aTimes < curve->maxTimes)
    {
        CalCurveSigmoid(curve);
        curve->aTimes++;
    }
    else
    {
        curve->currentSpeed = curve->targetSpeed;
        curve->maxTimes = 0;
        curve->aTimes = 0;
    }
}
/**
 * @brief  S型曲线速度计算
 * @param {CurveObjectType} *curve
 * @return {*}
 */
void CalCurveSigmoid(CurveObjectType *curve)
{
    float power = 0.0f, speed = 0.0f;
    /* v_cur = v_start + (v_target - v_start) / (1-exp(-flexible*((2t-T_max)/T_max))) */
    power = (2.0f * ((float)curve->aTimes) - ((float)curve->maxTimes)) / ((float)curve->maxTimes);
    power = (0.0f - curve->flexible) * power;
    speed = 1 + expf(power);
    speed = (curve->targetSpeed - curve->startSpeed) / speed;
    curve->currentSpeed = curve->startSpeed + speed;
    curve->currentSpeed = MaxMinLimit(curve->currentSpeed, curve->speedMax);
}